#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Sensor, S2,     HTSMUX,  sensorI2CCustom)
#pragma config(Sensor, S3,     HTSPB,   sensorI2CCustom9V)
#pragma config(Motor,  mtr_S1_C1_1,     linearSlides,  tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     grabberMount,  tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     frontLeft,     tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     rearLeft,      tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C3_1,     frontRight,    tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C3_2,     rearRight,     tmotorTetrix, openLoop, reversed)
#pragma config(Servo,  srvo_S1_C4_1,    wristMain,            tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    grabberRight,         tServoStandard)
#pragma config(Servo,  srvo_S1_C4_3,    grabberLeft,          tServoStandard)
#pragma config(Servo,  srvo_S1_C4_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_5,    irPivot,              tServoStandard)
#pragma config(Servo,  srvo_S1_C4_6,    rampLatch,            tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*   HiTechnic Experimenter's Kit for the SuperPro

Experiment - 2 Six LEDs and a Potentiometer

This program reads the analog value of a potentiometer and outputs a
digital value to one of six LEDs.

*/

#include "drivers/hitechnic-superpro.h"
const int rMax = 1023;
const int rMin = 407;
task main() {
  int inputdata;
  ubyte outputdata;
  int bit;

  // Set B0 as output
  HTSPBsetupIO(HTSPB, 0x1);

  while(true) {
    // Read a 10bit wide analogue value from A0
    inputdata = HTSPBreadADC(HTSPB, 2, 10);

    float wrist = (inputdata - rMin);
    wrist /= (rMax - rMin);
    servo [wristMain] = 255 * wrist;
    nxtDisplayTextLine(1, "A2: %d", inputdata);
    nxtDisplayTextLine(3, "W: %1.2f", wrist);

    // Set the output bit based on the analogue input value
    bit = (inputdata/128);
    if (bit > 5) bit = 5;
    outputdata = 1 << bit;

    HTSPBwriteIO(HTSPB, outputdata);
    wait1Msec(50);
  }
}
